# -*- coding: utf-8 -*-
"""
Created on Wed Nov 13 13:54:18 2024

@author: ZHOU
"""

# -*- coding: utf-8 -*-
"""
Created on Tue Nov 12 16:51:00 2024

@author: ZHOU
"""

import matplotlib.pyplot as plt


def Recursive(now_z,last_x,k):
    now_x = last_x + (now_z - last_x)/k
    return now_x

x = [85.6,84.3,84.0,86.5,85.5,85.0,84.8,84.5,84.5,85.1,85.2,84.4,85.0,86.1,85.2,85.5,84.9,84.8,84.5,85.3];
a=[]
b=[]
c=[]
d=[]

last_x = x[0]
for i in range(len(x)):
    now_x = Recursive(x[i],last_x,i+1)
    c.append(now_x)
    last_x = now_x
    
plt.figure(1)
plt.plot(x,color="g")
plt.plot(c,color="b")

def kalman(measure,result_last=0,prediction_last=0,Q=0.018,R=0.0542):
    result_mid = result_last
    prediction_mid = prediction_last + Q
    kg = prediction_mid/(prediction_mid + R)
    result_now = result_mid + kg*(measure - result_mid)
    prediction_now = (1-kg)*prediction_mid
    prediction_last = prediction_now
    result_last = result_now
    return result_now,result_last,prediction_last

def Kalman_Recursive(measure,evaluation_last,e_MEA_now,e_EST_last):
    kg =  e_EST_last/(e_EST_last+e_MEA_now)
    evaluation_now = evaluation_last + kg*(measure-evaluation_last)
    e_EST_now = (1-kg)*e_EST_last
    return evaluation_now,e_EST_now

e_MEA = 1.5
last_e_EST = 1
last_x = 85


for i in range(len(x)):
    now_x,now_e_EST= Kalman_Recursive(x[i],last_x,e_MEA,last_e_EST)
    b.append(now_x)
#    print(now_x)
    last_x = now_x    
    last_e_EST = now_e_EST


plt.plot(b,color="r")

e_MEA = 1.5
last_e_EST = 1
last_x = 85
pre_last = 85

for i in range(len(x)):
    now_x,last_x,pre_last= kalman(x[i],last_x,pre_last,1,1.5)
    d.append(now_x)
plt.plot(d,color="y")

a=[]
b=[]
c=[]
d=[]

x = [85.6,84.3,84.0,86.5,85.5,85.0,84.8,84.5,84.5,85.1,85.2,84.4,85.0,86.1,85.2,85.5,84.9,84.8,84.5,85.3];

y = [84.6,83.6,83.2,85.1,84.3,84.2,83.7,83.5,83.0,84.5,84.3,83.4,84.2,85.3,84.7,84.1,83.6,83.5,83.6,84.5];

m = [84.6,85.3,83.0,87.5,84.5,86.0,83.8,85.5,83.5,86.1,85.2,85.4,84.0,85.1,84.2,86.5,85.9,83.8,83.5,86.3];

plt.figure(2)
plt.plot(x,color="g")     
plt.plot(y,color="b")  

def Data_Fusion(z1,z2,s1,s2):
    kg = s1/(s1+s2)
    z=z1+kg*(z2-z1)
    s=(1-kg)*(1-kg)*s1+kg*kg*s2
    return z,s
    
for i in range(len(x)):
    z,s = Data_Fusion(x[i],y[i],2,1.5)
    a.append(z)
plt.plot(a,color="r")  


for i in range(len(a)):
    z,s = Data_Fusion(a[i],m[i],0.85,3)
    b.append(z)
plt.figure(3)
plt.plot(a,color="g")     
plt.plot(m,color="b")  
plt.plot(b,color="r") 

v = [10.5,20.6,30.8,40.6,45.3,48.0,47.5,44.5,46.0,43.8,55.5,53.5,56.1,65.2,67.6,68.9,72.2,80.1,81.5,82.6,83.4,84.6,83.5,83.1,85.0,84.5,84.0,83.6,83.9,83.2,84.1,85.6,84.3,84.0,86.5,85.5,85.0,84.8,84.5,84.5,85.1]

a=[]
b=[]
c=[]
d=[]

def Prediction(measure,result_last=0,covariance_last=0,Q=0.018,R=0.0542):
    result_priori = result_last
    covariance_priori = covariance_last + Q
    kg = covariance_priori/(covariance_priori + R)
    result_last = result_priori + kg*(measure - result_priori)
    covariance_last = (1-kg)*covariance_priori

    return result_last,covariance_last

result_last = v[0]
covariance_last = 0

for i in range(len(v)):
    result_last,covariance_last = Prediction(v[i],result_last,covariance_last,5,3)
    a.append(result_last)
    print(result_last)

plt.figure(4)
plt.plot(v,color="g")     
plt.plot(a,color="b")  

